#include "mainwindow.h"
#include "ui_mainwindow.h"

// 旋转方向，初始为顺时针
int dr = 0;
float input_pos;
auto input_pos_data = std_msgs::msg::Float32();
auto input_vel_data = std_msgs::msg::Float32();
auto flag = std_msgs::msg::Int32();


MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{

    ui->setupUi(this);

    // 设置文本框默认值
    ui->warningButton->setCheckable(true);

    // imu
    imuDataThread = new ImuDataThread(this);
    imuDataThread->start();
    // speed反馈
    speedDataThread = new SpeedDataThread(this);
    speedDataThread->start();
    // camera_1
    camera1DataThread = new Camera1DataThread(this);
    camera1DataThread->start();
    // camera_2
    camera2DataThread = new Camera2DataThread(this);
    camera2DataThread->start();
    // camera_3
    camera3DataThread = new Camera3DataThread(this);
    camera3DataThread->start();
    // 拉力传感器
    weighDataThread = new WeighDataThread(this);
    weighDataThread->start();
    // 温度传感器
    temDataThread = new TemDataThread(this);
    temDataThread->start();


    // 实例化Qprocess构造函数
    keyboard = new QProcess(this);
    driver = new QProcess(this);

    // imu信号到一个槽函数
    connect(imuDataThread,&ImuDataThread::imuEmitData,this,&MainWindow::display_imu);
    // weigh信号
    connect(weighDataThread,&WeighDataThread::weigh1_emit,this,&MainWindow::display_weigh1);
    connect(weighDataThread,&WeighDataThread::weigh2_emit,this,&MainWindow::display_weigh2);
    // 速度反馈
    connect(speedDataThread,&SpeedDataThread::speedEmitData1,this,&MainWindow::displayrt1data);
    connect(speedDataThread,&SpeedDataThread::speedEmitData2,this,&MainWindow::displayrt2data);
    connect(speedDataThread,&SpeedDataThread::speedEmitData3,this,&MainWindow::displayrt3data);
    connect(speedDataThread,&SpeedDataThread::speedEmitData4,this,&MainWindow::displayrt4data);
    // 温度反馈
    connect(temDataThread,&TemDataThread::tem_emit,this,&MainWindow::display_tem);
    // distance
    connect(temDataThread,&TemDataThread::distance_emit,this,&MainWindow::display_distance);
    // armpos
    connect(temDataThread,&TemDataThread::armpos_emit,this,&MainWindow::display_pos);
    connect(temDataThread,&TemDataThread::armpos_emit_2,this,&MainWindow::display_pos_2);
    // 漆膜仪反馈
    connect(temDataThread,&TemDataThread::thickness_emit,this,&MainWindow::display_thick);
    // 辅助轮推杆电压
    connect(temDataThread,&TemDataThread::wheel_voltage_emit,this,&MainWindow::display_v1);
    connect(temDataThread,&TemDataThread::arm_voltage_emit,this,&MainWindow::display_v2);
    connect(temDataThread,&TemDataThread::wheel_status_emit,this,&MainWindow::display_v1_colcor);
    connect(temDataThread,&TemDataThread::arm_status_emit,this,&MainWindow::display_v2_colcor);
    // 巡航或遥杆模式
    connect(temDataThread,&TemDataThread::joystick_or_cruising_emit,this,&MainWindow::display_jor_cur_colcor);
    // 巡航速度
    connect(temDataThread,&TemDataThread::cruising_speed_emit,this,&MainWindow::display_cur_speed);

    // camera1信号到一个槽函数
    connect(camera1DataThread,&Camera1DataThread::image_1Emit,this,&MainWindow::display_camera1,Qt::BlockingQueuedConnection);
    // camera2信号到一个槽函数
    connect(camera2DataThread,&Camera2DataThread::image_2Emit,this,&MainWindow::display_camera2,Qt::BlockingQueuedConnection);
    // camera3信号到一个槽函数
    connect(camera3DataThread,&Camera3DataThread::image_3Emit,this,&MainWindow::display_camera3,Qt::BlockingQueuedConnection);
    // // 测试label显示图片
    // QImage *img = new QImage;
    // img->load("/home/zhangchenxu/Pictures/摄像头/01.jpg");
    // ui->camera1Label->setPixmap(QPixmap::fromImage(*img));

}

MainWindow::~MainWindow()
{

    rclcpp::shutdown();
    delete ui;

}
// pos
void MainWindow::display_pos(float data)
{
    ui->arm_pos->setStyleSheet("QLineEdit {}");
    ui->arm_pos->setReadOnly(true);
    ui->arm_pos->setText(QString::number(data));
}
void MainWindow::display_pos_2(float data)
{
    ui->arm_pos_2->setStyleSheet("QLineEdit {}");
    ui->arm_pos_2->setReadOnly(true);
    ui->arm_pos_2->setText(QString::number(data,'f',1));
}
// 巡航速度显示
void MainWindow::display_cur_speed(float sp)
{
    ui->CruisingSpeed->setStyleSheet("QLineEdit {}");
    ui->CruisingSpeed->setReadOnly(true);
    ui->CruisingSpeed->setText(QString::number(sp,'f',1));
}
// 温度显示
void MainWindow::display_tem(int tem)
{
    // 设置输出框无光标且只读
    ui->tem_play->setStyleSheet("QLineEdit {}");
    ui->tem_play->setReadOnly(true);
    ui->tem_play->setText(QString::number(tem));
}
void MainWindow::display_distance(float data)
{
    ui->distance_play->setStyleSheet("QLineEdit {}");
    ui->distance_play->setReadOnly(true);
    ui->distance_play->setText(QString::number(data));
}
void MainWindow::display_thick(int thick)
{
    ui->thick_play->setStyleSheet("QLineEdit {}");
    ui->thick_play->setReadOnly(true);
    ui->thick_play->setText(QString::number(thick));
}

void MainWindow::display_v1(float v1)
{
    ui->voltage_wheel->setStyleSheet("QLineEdit {}");
    ui->voltage_wheel->setText(QString::number(v1,'f',1));
}
void MainWindow::display_v2(float v2)
{
    ui->arm_voltage->setStyleSheet("QLineEdit {}");
    ui->arm_voltage->setText(QString::number(v2,'f',1));
}
void MainWindow::display_v1_colcor(int v1_c)
{
    if(v1_c == 1)
    {
        ui->downButton->setStyleSheet("background-color: green;");
    }
    else if(v1_c == 2)
    {
        ui->pushButton->setStyleSheet("background-color: green;");
    }
    else
    {
        ui->downButton->setStyleSheet("");
        ui->pushButton->setStyleSheet("");
    }
}

void MainWindow::display_v2_colcor(int v2_c)
{
    if(v2_c == 2)
    {
        ui->armdownButton->setStyleSheet("background-color: green;");
    }
    else if(v2_c == 1)
    {
        ui->armpushButton->setStyleSheet("background-color: green;");
    }
    else
    {
        ui->armdownButton->setStyleSheet("");
        ui->armpushButton->setStyleSheet("");
    }
}
void MainWindow::display_jor_cur_colcor(int c)
{
    if(c == 0)
    {
        ui->joyButton->setStyleSheet("background-color: green;");
        ui->cruisingButton->setStyleSheet("");
    }
    else if(c == 1)
    {
        ui->cruisingButton->setStyleSheet("background-color: green;");
        ui->joyButton->setStyleSheet("");
    }
}

void MainWindow::displayrt1data(float turespeed1)
{
    // 设置输出框无光标且只读
    ui->lf_speed->setStyleSheet("QLineEdit {}");
    ui->lf_speed->setReadOnly(true);
    // 保留两位小数
    std::stringstream ss_speed1;
    ss_speed1 << std::fixed << std::setprecision(1) << turespeed1;
    float rt1 = std::stof(ss_speed1.str());
    // 显示在UI中
    ui->lf_speed->setText(QString::number(rt1));
}
void MainWindow::displayrt2data(float turespeed2)
{
    // 设置输出框无光标且只读
    ui->rf_speed->setStyleSheet("QLineEdit {}");
    ui->rf_speed->setReadOnly(true);
    // 保留两位小数
    std::stringstream ss_speed1;
    ss_speed1 << std::fixed << std::setprecision(1) << turespeed2;
    float rt1 = std::stof(ss_speed1.str());
    // 显示在UI中
    ui->rf_speed->setText(QString::number(rt1));
}
void MainWindow::displayrt3data(float turespeed3)
{
    // 设置输出框无光标且只读
    ui->lb_speed->setStyleSheet("QLineEdit {}");
    ui->lb_speed->setReadOnly(true);
    // 保留两位小数
    std::stringstream ss_speed1;
    ss_speed1 << std::fixed << std::setprecision(1) << turespeed3;
    float rt1 = std::stof(ss_speed1.str());
    // 显示在UI中
    ui->lb_speed->setText(QString::number(rt1));
}
void MainWindow::displayrt4data(float turespeed4)
{
    // 设置输出框无光标且只读
    ui->rb_speed->setStyleSheet("QLineEdit {}");
    ui->rb_speed->setReadOnly(true);
    // 保留两位小数
    std::stringstream ss_speed1;
    ss_speed1 << std::fixed << std::setprecision(1) << turespeed4;
    float rt1 = std::stof(ss_speed1.str());
    // 显示在UI中
    ui->rb_speed->setText(QString::number(rt1));
}





// void MainWindow::on_driverButton_clicked()
// {
//     driver->start("terminator",
//                   QStringList()<< "-e" <<
//                       "cd Documents/Wall-climbing/ros_driver && . install/setup.bash && ros2 run ros_driver ros_can_driver_node");
// }

// void MainWindow::on_wheelPushButton_clicked()
// {
//     wheel->start("terminator",
//                  QStringList()<< "-e" <<
//                      "cd Documents/Wall-climbing/ros_to_32 && . install/setup.bash && ros2 run ros_to_stm32 lis");
// }

void MainWindow::display_imu(double dis_r,double dis_p,double dis_y)
{
    // 设置imu的输出框无光标且只读
    ui->rollDisplay->setStyleSheet("QLineEdit {}");
    ui->rollDisplay->setReadOnly(true);
    ui->pitchDisplay->setStyleSheet("QLineEdit {}");
    ui->pitchDisplay->setReadOnly(true);
    ui->yawDisplay->setStyleSheet("QLineEdit {}");
    ui->yawDisplay->setReadOnly(true);
    // 保留两位小数
    std::stringstream ss_r, ss_p, ss_y;
    ss_r << std::fixed << std::setprecision(2) << dis_r;
    ss_p << std::fixed << std::setprecision(2) << dis_p;
    ss_y << std::fixed << std::setprecision(2) << dis_y;

    float r = std::stof(ss_r.str());
    float p = std::stof(ss_p.str());
    float y = std::stof(ss_y.str());
    // 显示在UI中
    ui->rollDisplay->setText(QString::number(r));
    ui->pitchDisplay->setText(QString::number(p));
    ui->yawDisplay->setText(QString::number(y));
}

void MainWindow::display_weigh1(float dis_w1)
{
    ui->l_push->setStyleSheet("QLineEdit {}");
    ui->l_push->setReadOnly(true);
    // 显示在UI中
    ui->l_push->setText(QString::number(dis_w1,'f',1));
}
void MainWindow::display_weigh2(float dis_w2)
{
    ui->r_push->setStyleSheet("QLineEdit {}");
    ui->r_push->setReadOnly(true);
    // 显示在UI中
    ui->r_push->setText(QString::number(dis_w2,'f',1));
}


void MainWindow::display_camera1(cv::Mat img1)
{
    cv::Mat rgb1;
    QImage qimg1;
    cv::cvtColor(img1, rgb1, CV_BGR2RGB);
    qimg1 = QImage((const unsigned char*)(rgb1.data),
                   rgb1.cols, rgb1.rows,
                   rgb1.cols*rgb1.channels(),
                   QImage::Format_RGB888);

    ui->camera1Label->setPixmap(QPixmap::fromImage(qimg1));
    // ui->camera1Label->setScaledContents(true); //开启后无法正常显示？

}

void MainWindow::display_camera2(cv::Mat img2)
{
    cv::Mat rgb2;
    QImage qimg2;
    cv::cvtColor(img2, rgb2, CV_BGR2RGB);
    qimg2 = QImage((const unsigned char*)(rgb2.data),
                   rgb2.cols, rgb2.rows,
                   rgb2.cols*rgb2.channels(),
                   QImage::Format_RGB888);

    ui->camera2Label->setPixmap(QPixmap::fromImage(qimg2));
}
void MainWindow::display_camera3(cv::Mat img3)
{
    cv::Mat rgb3;
    QImage qimg3;
    cv::cvtColor(img3, rgb3, CV_BGR2RGB);
    qimg3 = QImage((const unsigned char*)(rgb3.data),
                   rgb3.cols, rgb3.rows,
                   rgb3.cols*rgb3.channels(),
                   QImage::Format_RGB888);

    ui->camera3Label->setPixmap(QPixmap::fromImage(qimg3));
}















